#ifndef RINGBUFFER_H
#define RINGBUFFER_H
#include <cstdint>
//#define ringbufferlenght 1024
struct RingBuffer {
  bool dataReady{};

  uint8_t *m_buffer;
  volatile uint64_t m_head;
  volatile uint64_t m_tail;
  uint16_t m_lenght;
  explicit RingBuffer(uint8_t *buffer, uint16_t lenght) : m_buffer(buffer), m_head(0), m_tail(0), m_lenght(lenght) {
    memset(m_buffer, 0, lenght);
  }

  uint8_t save(uint8_t *data, size_t lenght) {

//    __disable_irq();
    if (m_head - m_tail > m_lenght - lenght) {

      return 0;
    }
    if (lenght > m_lenght - (m_head % m_lenght)) {
      memcpy(&m_buffer[m_head % m_lenght], data, m_lenght - (m_head % m_lenght));
      memcpy(&m_buffer[0], data + m_lenght - (m_head % m_lenght), lenght - m_lenght + (m_head % m_lenght));
    } else {
      memcpy(&m_buffer[m_head % m_lenght], data, lenght);
    }
    m_head += lenght;
//    __enable_irq();
    return 1;
  }

  uint64_t get(uint8_t *data, size_t lenght = 0) {

//    __disable_irq();
    uint64_t datalen = 0;
    if (m_head <= m_tail) {
      return 0;
    }

    if (lenght == 0 || m_head - m_tail < lenght) {
      datalen = m_head - m_tail;
    } else {
      datalen = lenght;
    }
    if (datalen > m_lenght - (m_tail % m_lenght)) {
      memcpy(data, &m_buffer[m_tail % m_lenght], m_lenght - (m_tail % m_lenght));
      memcpy(data + m_lenght - (m_tail % m_lenght), &m_buffer[0], datalen - m_lenght + (m_tail % m_lenght));
    } else {
      memcpy(data, &m_buffer[m_tail % m_lenght], datalen);
    }
    m_tail += datalen;
//    __enable_irq();
    return datalen;
  }

  [[nodiscard]] uint64_t head() const {
    return m_head;
  }
  [[nodiscard]] uint64_t tail() const{
    return m_tail;
  }
  [[nodiscard]] uint16_t length() const {
    if (m_head >= m_tail) {
      return m_head - m_tail;
    } else {
      return 0;
    }
  }

  [[nodiscard]] uint8_t *data() const {
    return m_buffer;
  }
};
#pragma pack(1)

struct ACCRawData {
  float acc_x;
  float acc_y;
  float acc_z;
};
struct GYRORawData {
  float gyro_x;
  float gyro_y;
  float gyro_z;
};
struct MAGRawData {
  float mag_x;
  float mag_y;
  float mag_z;
};

struct GPSRawData {
  uint32_t latitude;
  uint32_t longitude;
  float velocity;
};
struct ATTITUDEQuaternionData {
  float Q0;
  float Q1;
  float Q2;
  float Q3;
};

#pragma pack()

template<typename T>
struct RingBufferT {

  bool dataReady{};

  //    std::mutex mutex;
  uint8_t *m_buffer;
  volatile uint64_t m_head;
  volatile uint64_t m_tail;
  uint64_t m_lenght;
  explicit RingBufferT(uint8_t *buffer, uint16_t count) : m_buffer(buffer), m_head(0), m_tail(0), m_lenght(count * sizeof(T)) {
    memset(m_buffer, 0, sizeof(m_buffer));
  }

  // no copy, assignment, move, move assignment
  RingBufferT(const RingBufferT &) = delete;
  RingBufferT &operator=(const RingBufferT &) = delete;
  RingBufferT(RingBufferT &&) = delete;
  RingBufferT &operator=(RingBufferT &&) = delete;

  uint8_t save(T &data) {
    size_t lenght = sizeof(T);

    if (m_head - m_tail > m_lenght - lenght) {
      return 0;
    }
    if (lenght > m_lenght - (m_head % m_lenght)) {
      memcpy(&m_buffer[m_head % m_lenght], &data, m_lenght - (m_head % m_lenght));
      memcpy(&m_buffer[0], &data + m_lenght - (m_head % m_lenght), lenght - m_lenght + (m_head % m_lenght));
    } else {
      memcpy(&m_buffer[m_head % m_lenght], &data, lenght);
    }
    m_head += lenght;
    return 1;
  }
  uint8_t save(T &data, size_t count) {
    size_t lenght = count * sizeof(T);

    if (m_head - m_tail > m_lenght - lenght) {
      return 0;
    }
    if (lenght > m_lenght - (m_head % m_lenght)) {
      memcpy(&m_buffer[m_head % m_lenght], &data, m_lenght - (m_head % m_lenght));
      memcpy(&m_buffer[0], &data + m_lenght - (m_head % m_lenght), lenght - m_lenght + (m_head % m_lenght));
    } else {
      memcpy(&m_buffer[m_head % m_lenght], &data, lenght);
    }
    m_head += lenght;
    return 1;
  }

  uint64_t get(uint8_t *data, size_t count) {
    size_t lenght = count * sizeof(T);
    uint64_t datalen;
    if (m_head <= m_tail)
      return 0;
    if (lenght == 0 || m_head - m_tail < lenght) {
      datalen = m_head - m_tail;
    } else {
      datalen = lenght;
    }
    if (datalen > m_lenght - (m_tail % m_lenght)) {
      memcpy(data, &m_buffer[m_tail % m_lenght], m_lenght - (m_tail % m_lenght));
      memcpy(data + m_lenght - (m_tail % m_lenght), &m_buffer[0], datalen - m_lenght + (m_tail % m_lenght));
    } else {
      memcpy(data, &m_buffer[m_tail % m_lenght], datalen);
    }
    m_tail += datalen;
    return datalen;
  }
  uint64_t head() {
    return m_head;
  }
  uint64_t tail() {
    return m_tail;
  }
  uint64_t length() {
    if (m_head >= m_tail) {
      return (m_head - m_tail);
    } else {
      return 0;
    }
  }

  uint64_t size() {
    if (m_head >= m_tail) {
      return (m_head - m_tail)/sizeof(T);
    } else {
      return 0;
    }
  }

  uint8_t *data() {
    return m_buffer;
  }

  void push_front(T &item) {
    save(item);
  }

  bool pop_back(T *pItem) {
    size_t lenght = sizeof(T);
    uint64_t datalen;
    if (m_head <= m_tail)
      return false;
    if (m_head - m_tail < lenght) {
      return false;
    } else {
      datalen = lenght;
    }
    if (datalen > m_lenght - (m_tail % m_lenght)) {
      memcpy(pItem, &m_buffer[m_tail % m_lenght], m_lenght - (m_tail % m_lenght));
      memcpy(pItem + m_lenght - (m_tail % m_lenght), &m_buffer[0], datalen - m_lenght + (m_tail % m_lenght));
    } else {
      memcpy(pItem, &m_buffer[m_tail % m_lenght], datalen);
    }
    m_tail += datalen;
    return true;
  }
  bool back(T *pItem) {
    size_t lenght = sizeof(T);
    uint64_t datalen;
    if (m_head <= m_tail)
      return false;
    if (m_head - m_tail < lenght) {
      return false;
    } else {
      datalen = lenght;
    }
    if (datalen > m_lenght - (m_tail % m_lenght)) {
      memcpy(pItem, &m_buffer[m_tail % m_lenght], m_lenght - (m_tail % m_lenght));
      memcpy(pItem + m_lenght - (m_tail % m_lenght), &m_buffer[0], datalen - m_lenght + (m_tail % m_lenght));
    } else {
      memcpy(pItem, &m_buffer[m_tail % m_lenght], datalen);
    }
    // tail+=datalen;
    return true;
  }

  bool is_not_empty() {
    return m_head > m_tail;
  }
};

#endif // RINGBUFFER_H
